#include <arduino.h>
#include <Wire.h>
 extern "C" void __cxa_pure_virtual(void) {
    while(1);
} 
//#include "TimerEvenetQueue.h"
#include "drivetrain.h"
long unsigned minTime(5200);
int speed1(0),speed2(0);
#define ENC1_INT 0                     // INT0 on digital pin 2
#define ENC2_INT 1                     // INT1 on digital pin 3
unsigned long time;
int counter;
#define address 0x1E
TimerQueue timer;
Drivetrain drive(timer);


void setup() {
  Serial.begin(9600);                 // create the serial connection at 9600bit/s baudrate
  Wire.begin();                       // initilize I2C communication with the Arduino as master

  //Put the HMC5883 IC into the correct operating mode
  Wire.beginTransmission(address); //open communication with HMC5883
  Wire.write(0x02); //select mode register
  Wire.write(0x00); //continuous measurement mode
  Wire.endTransmission();
}
void compass()
{
  static long unsigned int timestamp = 0;
  if(millis() - timestamp >= 50)
  {
    int x,y,z; //triple axis data
    //Tell the HMC5883 where to begin reading data
    Wire.beginTransmission(address);
    Wire.write(0x03); //select register 3, X MSB register
    Wire.endTransmission();
   //Read data from each axis, 2 registers per axis
    Wire.requestFrom(address, 6);
    if(6<=Wire.available()){
      x = Wire.read()<<8; //X msb
      x |= Wire.read(); //X lsb
      z = Wire.read()<<8; //Z msb
      z |= Wire.read(); //Z lsb
      y = Wire.read()<<8; //Y msb
      y |= Wire.read(); //Y lsb
    }
    int bearing = 180.*atan2(x,y)/M_PI+180;
    Serial.print("compass brearing: ");Serial.println(bearing);
    drive.setCurrentBearing(bearing);
    timestamp = millis();
  }
}
void loop()
{
    if(Serial.available())
      {
        char data = Serial.read();
        switch(data)
        {
         case 'a':
          {
            drive.turnleft();
            break;
          }
         case 'x':
          {
            drive.stop();
            break;
          }
         case 'd':
         {
            drive.turnright();
            break;
         }
         case 'w':
         {
            drive.faster();
            break;
         }
         case 's':
         {
            drive.slower();
            break;
         }
        }
    }
    compass();
    timer.periodicTimerTick();
}
int main()
{
    setup();
    while(true)
        loop();
    return 0;
}

